#include "subctrl.h"
#include "phase.h"
#include "pid.h"
#include "sys.h"
#include "tim.h"
#include <cmath>
#include <cstdint>
#include <cstring>
#include <string.h>

void SubController::Init(const pa_module_hardware_config_t *config)
{
    LOG->Print("\n开始初始化子控制器\n");
    if (handle_->init != DEFAULT_INITIAL_VALUE) /* 硬件复位 */
    {
        memset(handle_, 0, sizeof(sub_control_t));
        handle_->init = DEFAULT_INITIAL_VALUE;
        handle_->state.pa_enable = PA_MODULE_DISABLE_BIT;
        handle_->frame_header = RS485_FRAME_HEADER;
        handle_->flash.config.att_val = ATTENUATION_MAX;
        handle_->flash.config.phase_val = PHASE_MAX / 2;
        handle_->state.soft_reset_count = 0;
        LOG->Print("硬件复位，参数已初始化\n");
        /* 配置PA模块 */
        for (uint8_t i = 0; i < kModuleNum; i++)
        {
            handle_->pa[i].hardware = &config[i];
        }
    }
    else /* 软件复位 */
    {
        handle_->state.soft_reset_count++;
        LOG->Print("发生软件复位：%d次\n", handle_->state.soft_reset_count);
    }
    LOG->Print("控制器状态：%s\n", GetStatus());
    Flash_Init(&handle_->flash);
    SetAttenuation(handle_->flash.config.att_val);
    SetPhase(handle_->flash.config.phase_val);
    if (handle_->state.pa_enable != PA_MODULE_ENABLE_BIT)
        OutputDisable();
    // 加载pid参数
    PID_Init(&forward_pid_, &handle_->flash.config.forward_pid);
    PID_Init(&reverse_pid_, &handle_->flash.config.reverse_pid);
    LOG->Print("子控制器初始化完成\n");
}

void SubController::ForwardLoop()
{
    if (handle_->state.auto_run != kON && handle_->state.pa_enable != PA_MODULE_ENABLE_BIT)
    {
        PID_Clear(&forward_pid_);
        return;
    }
    if (handle_->rf_power.reverse > REVERSE_ERROR_THRESHOLD)
        return;
    forward_pid_.error = handle_->rf_power.target - handle_->rf_power.forward;
    handle_->flash.config.att_val -= PID_Update(&forward_pid_);
    SetAttenuation(handle_->flash.config.att_val);
}
void SubController::ReverseLoop()
{
    static int8_t phase_dir = 1;
    static uint8_t anti_count = 0;

    if (handle_->state.auto_run != kON && handle_->state.pa_enable != PA_MODULE_ENABLE_BIT)
    {
        PID_Clear(&reverse_pid_);
    }
    /* 反向功率环 */
    if (reverse_pid_.error > reverse_pid_.error_prev) // 改变相位方向
    {
        anti_count++;
        if (anti_count >= PHASE_DIR_ANTI_COUNT)
        {
            anti_count = 0;
            phase_dir *= -1;
        }
    }
    else
    {
        anti_count = 0;
    }
    reverse_pid_.error = handle_->rf_power.reverse * phase_dir;
    handle_->flash.config.phase_val += PID_Update(&reverse_pid_);
    SetPhase(handle_->flash.config.phase_val);
}

void SubController::OutputEnable()
{
    handle_->state.pa_enable = PA_MODULE_ENABLE_BIT;

    OutputBitCtrl(handle_->state.pa_enable);
    LOG->Print("输出使能\n");
}

void SubController::OutputEnable(pa_module_id_t &id)
{
    if (id >= kModuleNum)
    {
        LOG->Print("模块ID: [%d] 超出范围\n", id);
        return;
    }
    BIT_SET(handle_->state.pa_enable, id);

    OutputBitCtrl(handle_->state.pa_enable);
    LOG->Print("PA[%d] 输出使能\n", id);
}

void SubController::OutputDisable()
{
    handle_->state.pa_enable = PA_MODULE_DISABLE_BIT;

    OutputBitCtrl(handle_->state.pa_enable);
    LOG->Print("输出失能\n");
}

void SubController::OutputDisable(pa_module_id_t &id)
{
    if (id >= kModuleNum)
    {
        LOG->Print("模块ID: [%d] 超出范围\n", id);
        return;
    }
    BIT_CLEAR(handle_->state.pa_enable, id);

    OutputBitCtrl(handle_->state.pa_enable);
    LOG->Print("PA[%d] 输出失能\n", id);
}

void SubController::SetTargetPower(float &value)
{
    if (value < 0.0f || value > TARGET_POWER_MAX)
    {
        LOG->Print("设置目标功率失败，超出范围\n");
        return;
    }
    handle_->rf_power.target = value;
    PID_Init(&forward_pid_, &handle_->flash.config.forward_pid);
    PID_Init(&reverse_pid_, &handle_->flash.config.reverse_pid);
    if (handle_->rf_power.target != 0)
    {
        LOG->Print("设置目标功率为 %5.0f W, 并使能输出\n", handle_->rf_power.target);
        OutputEnable();
    }
    else
    {
        LOG->Print("已失能输出\n");
        OutputDisable();
    }
}

void SubController::SetAttenuation(float &dB)
{
    if (dB < 0.0f || dB > ATTENUATION_MAX)
    {
        LOG->Print("设置衰减值失败，超出范围\n");
        return;
    }

    __HAL_TIM_SET_PRESCALER(&htim4, 72 - 1);
    __HAL_TIM_SET_AUTORELOAD(&htim4, TIM_AUTORELOAD - 1);
    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, (dB / ATTENUATION_MAX) * TIM_AUTORELOAD);

    HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_1);
    handle_->flash.config.att_val = dB;
    LOG->Print("设置衰减值为 %5.2f dB\n", handle_->flash.config.att_val);
}

void SubController::SetPhase(float &deg)
{
    uint16_t data;
    if (deg < 0.0f || deg > PHASE_MAX)
    {
        LOG->Print("设置相位值失败，超出范围\n");
        return;
    }

    data = (uint16_t)roundf(deg * (256 / 360.0f));
    handle_->flash.config.phase_val = Phase_Write(data);
    LOG->Print("设置相位值为 %5.2f 度\n", handle_->flash.config.phase_val);
}

void SubController::SetAddress(uint8_t &addr)
{
    if (addr < SUBCTRL_BASE_ADDRESS || addr > (SUBCTRL_BASE_ADDRESS + kSubCtrlNum - 1))
    {
        LOG->Print("设置IP地址失败! 地址应在 [%d, %d] 之间\n", SUBCTRL_BASE_ADDRESS,
                   SUBCTRL_BASE_ADDRESS + kSubCtrlNum - 1);
        return;
    }
    handle_->flash.ip[3] = addr;
    // clang-format off
    LOG->Print("设置IP地址为 %d.%d.%d.%d\n",
                            handle_->flash.ip[0],
                            handle_->flash.ip[1],
                            handle_->flash.ip[2],
                            handle_->flash.ip[3]);
    // clang-format on
}

void SubController::SetAutoRun(switch_state_t &state)
{
    handle_->state.auto_run = state;
    LOG->Print("设置自动运行状态为 %s\n", state ? "开启" : "关闭");
}

void SubController::SetForwardPID(pid_config_t &config)
{
    // Parameter check
    if (config.Kp > FORWARD_PID_KP_MAX || config.Ki > FORWARD_PID_KI_MAX || config.Kd > FORWARD_PID_KD_MAX ||
        config.output_max > FORWARD_PID_OUTPUT_MAX)
    {
        LOG->Print("设置正向PID参数失败,超出范围\n");
        return;
    }
    memcpy(&handle_->flash.config.forward_pid, &config, sizeof(pid_config_t));
    PID_Init(&forward_pid_, &handle_->flash.config.forward_pid);
    LOG->Print("已设置正向PID参数: \n");
    PrintPidParam(forward_pid_);
}

void SubController::SetReversePID(pid_config_t &config)
{
    // Parameter check
    if (config.Kp > REVERSE_PID_KP_MAX || config.Ki > REVERSE_PID_KI_MAX || config.Kd > REVERSE_PID_KD_MAX ||
        config.output_max > REVERSE_PID_OUTPUT_MAX)
    {
        LOG->Print("设置反向PID参数失败,超出范围\n");
        return;
    }
    memcpy(&handle_->flash.config.reverse_pid, &config, sizeof(pid_config_t));
    PID_Init(&reverse_pid_, &handle_->flash.config.reverse_pid);
    LOG->Print("已设置反向PID参数: \n");
    PrintPidParam(reverse_pid_);
}

char *SubController::GetStatus()
{
    static const char *status[] = {"正常", "异常", "重启"};
    return (char *)status[handle_->state.error];
}

uint8_t SubController::CheckAddress(uint8_t &addr)
{
    return handle_->flash.ip[3] == addr;
}

void SubController::PrintPidParam(pid_t &pid)
{
    LOG->Print("Kp = %8.6f\n"
               "Ki = %8.6f\n"
               "Kd = %8.6f\n"
               "output_max = %5.2f\n"
               "output_min = %5.2f\n"
               "error_total_threshold = %5.2f\n"
               "error = %5.2f\n"
               "error_prev = %5.2f\n"
               "error_total = %5.2f\n",
               pid.config.Kp, pid.config.Ki, pid.config.Kd, pid.config.output_max, pid.config.output_min,
               pid.config.error_total_threshold, pid.error, pid.error_prev, pid.error_total);
}

void SubController::SaveParameter()
{
    // clang-format off
    if(Flash_Write(FLASH_START_ADDR,
                   (uint32_t *)&handle_->flash,
                   sizeof(sub_control_flash_t) / sizeof(uint32_t)))
    {
        LOG->Print("参数保存失败\n");
    }
    else
    {
        LOG->Print("参数保存成功\n");
    }
    // clang-format on
}

void SubController::Restart(uint8_t &mode)
{
    if (handle_->state.pa_enable != PA_MODULE_DISABLE_BIT)
    {
        LOG->Print("请先关闭输出，再进行重启\n");
        return;
    }
    LOG->Print("正在重启\n");
    handle_->init = mode;
    delay_ms(200);
    System_Reset();
}

// Private functions

void SubController::OutputBitCtrl(uint8_t bit)
{
    HAL_GPIO_WritePin(M_PTT1_GPIO_Port, M_PTT1_Pin, bit ? GPIO_PIN_SET : GPIO_PIN_RESET);
    // clang-format off
    for (int8_t i = 0; i < kModuleNum; i++)
    {
        HAL_GPIO_WritePin(handle_->pa[i].hardware->pd_index->port,
                          handle_->pa[i].hardware->pd_index->pin,
                          (GPIO_PinState)BIT_READ(bit, i));
        delay_ms(20);
    }
    // clang-format on
}